One prior art terrain aided navigation system is available from Sandia Labs. Sandia has created the Sandia Inertial Terrain-Aided Navigation (SITAN) flight-computer algorithm that produces a very accurate trajectory for low-flying, high-performance aircraft by combining outputs from a radar or laser altimeter, an inertial navigation system (INS), and a digital terrain elevation map. SITAN is a recursive, real time, terrain-aided navigation algorithm for use on fighter aircraft. The algorithm has been implemented in a popular microprocessor. The aircraft's position can be estimated within a 926 meter circle error of probability. A good description of the SITAN terrain aided navigation system can be found in the proceedings of the IEEE National Aerospace and Electronics Conference--NAECON, May 20-24, 1985 entitled The AFTI/F16 Terrain-Aided Navigation System, by D. D. Boozer, M. K. Lau, J. R. Fellerhoff, Sandia National Laboratories, Albuquerque, N. Mex. 87185.
SITAN utilizes Kalman filter algorithms to process geophysical measurements. The algorithm estimates errors in a flight path produced by an inertial navigation system following the equations given in the above-mentioned article.
The Kalman filter can be formed with the following state model: and the measurement ##EQU1## where .delta.X.sub.k =INS error states to be estimated
.PHI.=state-transition matrix for INS errors PA1 X.sub.k =states of INS and aircraft PA1 C.sub.k =ground clearance measurement PA1 Z.sub.k =altitude of aircraft PA1 h=height of terrain at position (. , .) PA1 W.sub.k =driving noise with E(W.sub.k)=0 for all k and E(W.sub.k W.sub.j.sup.T)=Q.sub.k .delta..sub.kj PA1 V.sub.k =measurement error with E(V.sub.k)=0 for all k and E(V.sub.k V.sub.j)=R.sub.k .delta..sub.kj PA1 k=subscript denoting time k.
Since the measurement function c(x) is a nonlinear function of the states, the standard extended Kalman filter approach is used to obtain the measurement matrix, ##EQU2## where h.sub.x and h.sub.y are the terrain slopes in the x and y directions of the map evaluated at X.sub.k (-), the predicted aircraft position just before a measurement is processed at time k. The first three states are taken to be the x position, y position, and altitude, respectively. At any time k, EQU X=X.sub.INS +.delta.X
The state vector often used in a single filter implementation is EQU .delta.X=[.delta.X.delta.Y.delta.Z.delta.V.sub.X .delta.V.sub.Y ].sup.T
where .delta.X, .delta.Y, .delta.Z, .delta.V.sub.X, and .delta.V.sub.Y are errors in the X position, Y position, altitude, X velocity, and Y velocity, respectively. Other INS errors and states can also be included in .delta.X by using the proper .PHI..
Parallel SITAN was developed because the distance needed by SITAN to reach steady state becomes excessive as initial position errors (IPEs) approach several hundred meters. Parallel SITAN is a bank of extended Kalman filters that process identical altimeter measurements. After some updates, the filter with the minimum average weighted residual squared (AWRS) value is identified as having the correct position estimate. The AWRS value is defined by ##EQU3## where .DELTA..sub.i is the residual at the ith update, n is the number of updates, and HPH.sup.T +R is the residual variance. Once the IPEs are reduced by the parallel filters, a single filter performs well, starting off essentially in steady state.
To implement parallel SITAN, the number and geometrical layout of the parallel filters needed to cover an IPE must be specified. A square, constant-spaced grid can be used to center the filters about the horizontal position indicated by the INS. Filters at and near the corners are then eliminated to reduce the number of filters. To further lighten the computational burden, three-state, instead of five-state, filters are often used in parallel SITAN with EQU .delta.X=[.delta.X.delta.Y.delta.Z].sup.T
For both the single and parallel filter implementation, a least-squares plane fit to the map, known as stochastic linearization, is used to compute the slopes, h.sub.X and H.sub.Y. Horizontal uncertainties .sigma..sub.X and .sigma..sub.Y from the error-covariance matrix, defined by ##EQU4## are used to determine the size of the plane. Residuals from the plant fit, RFIT.sub.k, are added to the measurement error variance, R.sub.k, to ensure that the SITAN filter assigns less weight to the measurement when the plane fit is either very large or is over a rough area, thus adapting to local terrain.
SITAN has three basic modes: acquisition mode, lost mode and track mode.